/*
** LuaFileSystem
** Copyright Kepler Project 2003 - 2017 (http://keplerproject.github.io/luafilesystem)
*/

#include <Eigen/Eigen>
#include <Eigen/Eigenvalues>
#include <log/log.h>

template<class eigVector,class eigMatrix>
void sortEigenVectorByValues(eigVector& eigenValues, eigMatrix& eigenVectors) {

	using stdTupleEigen = std::tuple<std::complex<double>, eigMatrix>;
	using stdVectorTuple = std::vector<stdTupleEigen>;

	stdVectorTuple eigenValueAndVector;
	int size = static_cast<int>(eigenValues.size());

	eigenValueAndVector.reserve(size);
	for (int i = 0; i < size; ++i)
		eigenValueAndVector.push_back(stdTupleEigen(eigenValues[i], eigenVectors.col(i)));

	// 使用标准库中的sort，按从大到小排序
	std::sort(eigenValueAndVector.begin(), eigenValueAndVector.end(),
		[&](const stdTupleEigen& a, const stdTupleEigen& b) -> bool {
		return std::abs(std::get<0>(a)) > std::abs(std::get<0>(b));
		});

	for (int i = 0; i < size; ++i) {
		eigenValues[i] = std::get<0>(eigenValueAndVector[i]); // 排序后的特征值
		eigenVectors.col(i).swap(std::get<1>(eigenValueAndVector[i])); // 排序后的特征向量
	}
}

template<class eigVector,class eigMatrix>
void sortEigenVectorByValues(eigVector& eigenValues, eigMatrix& eigenVectors,std::string strSort) {

	using stdTupleEigen = std::tuple<std::complex<double>, eigMatrix>;
	using stdVectorTuple = std::vector<stdTupleEigen>;

	stdVectorTuple eigenValueAndVector;
	int size = static_cast<int>(eigenValues.size());

	eigenValueAndVector.reserve(size);
	for (int i = 0; i < size; ++i)
		eigenValueAndVector.push_back(stdTupleEigen(eigenValues[i], eigenVectors.col(i)));

	// 使用标准库中的sort，按从大到小排序
	if(strSort=="SM"){
		std::sort(eigenValueAndVector.begin(), eigenValueAndVector.end(),
			[&](const stdTupleEigen& a, const stdTupleEigen& b) -> bool {
			return std::abs(std::get<0>(a)) > std::abs(std::get<0>(b));
			});
	}else if(strSort=="LM"){
		std::sort(eigenValueAndVector.begin(), eigenValueAndVector.end(),
			[&](const stdTupleEigen& a, const stdTupleEigen& b) -> bool {
			return std::abs(std::get<0>(a)) < std::abs(std::get<0>(b));
			});
	}else if(strSort=="SR"){
		std::sort(eigenValueAndVector.begin(), eigenValueAndVector.end(),
			[&](const stdTupleEigen& a, const stdTupleEigen& b) -> bool {
			return std::real(std::get<0>(a)) > std::real(std::get<0>(b));
			});
	}else if(strSort=="LR"){
		std::sort(eigenValueAndVector.begin(), eigenValueAndVector.end(),
			[&](const stdTupleEigen& a, const stdTupleEigen& b) -> bool {
			return std::real(std::get<0>(a)) < std::real(std::get<0>(b));
			});
	}else if(strSort=="SI"){
		std::sort(eigenValueAndVector.begin(), eigenValueAndVector.end(),
			[&](const stdTupleEigen& a, const stdTupleEigen& b) -> bool {
			return std::imag(std::get<0>(a)) > std::imag(std::get<0>(b));
			});
	}else if(strSort=="LI"){
		std::sort(eigenValueAndVector.begin(), eigenValueAndVector.end(),
			[&](const stdTupleEigen& a, const stdTupleEigen& b) -> bool {
			return std::imag(std::get<0>(a)) < std::imag(std::get<0>(b));
			});
	}else{
		logs::warning_log(boost::format("arg error: %1%") % strSort);
		return;
	}

	for (int i = 0; i < size; ++i) {
		eigenValues[i] = std::get<0>(eigenValueAndVector[i]); // 排序后的特征值
		eigenVectors.col(i).swap(std::get<1>(eigenValueAndVector[i])); // 排序后的特征向量
	}
}


template<class T>
int solve_lu(Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& A,
			 const Eigen::Matrix<T, Eigen::Dynamic, 1>& b,
			 Eigen::Matrix<T, Eigen::Dynamic, 1>& x)
{
	typedef typename Eigen::Matrix<T, Eigen::Dynamic, 1> Vector;
	typedef typename Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Matrix;

	Eigen::FullPivLU<Matrix> solver;
	solver.compute(A);
	x = solver.solve(b);
	logs::info_log("solve lu");
	return 0;
}

template<class T>
int solve_lu(Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& A,
			 const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& B,
			 Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& X)
{
	typedef typename Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Matrix;

	Eigen::FullPivLU<Matrix> solver;
	solver.compute(A);
	X = solver.solve(B);
	return 0;
}


template<class T,class T2>
int solve_eig(Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& A,
			  Eigen::Matrix<T2, Eigen::Dynamic, 1>& v,
			  Eigen::Matrix<T2, Eigen::Dynamic, Eigen::Dynamic>& E,
			  bool sort=false)
{
	typedef typename Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Matrix;
	Eigen::EigenSolver<Matrix> solver;
	solver.compute(A);
	if(solver.info()!=Eigen::Success) {
	  return -1;
	}
	E = solver.eigenvectors();
	v = solver.eigenvalues();
	if(sort) sortEigenVectorByValues(v,E);
	return 0;
}

template<class T,class T2>
int solve_ceig(Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& A,
			  Eigen::Matrix<T2, Eigen::Dynamic, 1>& v,
			  Eigen::Matrix<T2, Eigen::Dynamic, Eigen::Dynamic>& E,
			  bool bSort=false)
{
	typedef typename Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Matrix;
	Eigen::ComplexEigenSolver<Matrix> solver;
	solver.compute(A);
	if(solver.info()!=Eigen::Success) {
	  return -1;
	}
	E = solver.eigenvectors();
	v = solver.eigenvalues();
	if(bSort) sortEigenVectorByValues(v,E);
	return 0;
}

template<class T,class T2>
int solve_geig(Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& A,
		       Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& B,
			   Eigen::Matrix<T2, Eigen::Dynamic, 1>& v,
			   Eigen::Matrix<T2, Eigen::Dynamic, Eigen::Dynamic>& E
			   )
{
	typedef typename Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Matrix;
	Eigen::GeneralizedEigenSolver<Matrix> solver;
	solver.compute(A,B);
	if(solver.info()!=Eigen::Success) {
	  return -1;
	}
	E = solver.eigenvectors();
	v = solver.eigenvalues();
	return 0;
}


template<class T,class T2>
int solve_gheig(Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& A,
		       Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& B,
			   Eigen::Matrix<T2, Eigen::Dynamic, 1>& v,
			   Eigen::Matrix<T2, Eigen::Dynamic, Eigen::Dynamic>& E)
{
	typedef typename Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Matrix;
	Eigen::GeneralizedSelfAdjointEigenSolver<Matrix> solver;
	solver.compute(A,B);
	if(solver.info()!=Eigen::Success) {
	  return -1;
	}
	E = solver.eigenvectors();
	v = solver.eigenvalues();
	return 0;
}
template<class T,class T2>
int solve_heig(Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& A,
			   Eigen::Matrix<T2, Eigen::Dynamic, 1>& v,
			   Eigen::Matrix<T2, Eigen::Dynamic, Eigen::Dynamic>& E )
{
	typedef typename Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Matrix;
	Eigen::SelfAdjointEigenSolver<Matrix> solver;
	solver.compute(A);
	if(solver.info()!=Eigen::Success) {
	  return -1;
	}
	E = solver.eigenvectors();
	v = solver.eigenvalues();
	return 0;
}
